/*
 * Neurologist.h
 *
 *  Created on: Feb 7, 2009
 *      Author: robotics
 *  
 */

#ifndef _NEUROLOGIST_H
#define _NEUROLOGIST_H

#include <iostream.h>
#include "math.h"

#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "TrackAPI.h" 
#include "Target.h" 
#include "WPILib.h"
#include "RobodoxTimer.h"
#include "RobodoxJaguar.h"
#include "RobodoxEncoder.h"
#include "NeuroAuto.hh"
#include "DriverStationLCD.h"


class Neurologist : public SimpleRobot
{
	
	Task updateTask;
	
	Joystick *driveStick;			// joystick 1 (drive)
	Joystick *liftStick;			// joystick 2 (roller)
	DriverStation *ds;				// driver station object
	DriverStationLCD *dsLCD;
	Servo *horizontalServo;  		// first servo object
	Servo *verticalServo;			// second servo object
	
	Jaguar *leftMotor;
	Jaguar *rightMotor;
	Relay *liftR;
	Relay *liftL; 
	Relay *agitator;
	Victor *rollerL;
	Victor *rollerR;
	
	DigitalInput *toplimitswitch;
	DigitalInput *bottomlimitswitch;
	
	DigitalInput *autoswitch1;
	DigitalInput *autoswitch2;
	//DigitalInput *allianceswitch;
	
	RobodoxTimer *time;
	
public:
	
	Neurologist();
	~Neurologist();
	
	Gyro *g1;
	Accelerometer *a_x;
	Accelerometer *a_y;
	
	RobodoxEncoder *leftGearbox;
	RobodoxEncoder *rightGearbox;
	RobodoxEncoder *rightRef;
	RobodoxEncoder *leftRef;
	
	NeuroAuto *Mr_Robauto;
	
	static const float w_radius = 3.0;  //main wheel radius
	static const float ref_radius = 2.75;  //radius of reference wheel
	
	float v_l; //  left side wheel speed inches per second
	float v_r;//  right side wheel speed ips
    float v_rref;// right reference wheel speed ips
    float v_lref;// left reference wheel speed ips
	float l_rate_rps;  //left wheel speed rps      
	float r_rate_rps;  //right wheel speed rps
	float dist_l;
	float dist_r;
	float dist_lref;
	float dist_rref;
                   
	float leftCmd_limit_pos;
	float rightCmd_limit_pos;
	float leftCmd_limit_neg;
	float rightCmd_limit_neg;
	float lastRightMotorCmd;
	float lastLeftMotorCmd;
	
	float v_avg;
	
	float heading;
	float headingRate;  //for the gyro
	
	float accelerationX;
	float accelerationY;
	float velocityX;
	float velocityY;	
	
	float xt, yt;
	float leftMotorCmd;
	float rightMotorCmd;
	float turn;
	float speed;
	bool cal_switch_port;
	float v_battery;
	float battery_factor;
	float v_r_slip;  
	float v_l_slip;
	bool sw_torqlim_on;
	
	
private:
	typedef struct robotDataStruct {
		float leftCmd_limit_pos;
		float rightCmd_limit_pos;
		float leftCmd_limit_neg;
		float rightCmd_limit_neg;
	};
    robotDataStruct *robotData;
	FILE *limitFile;
	
	void setServoPositions(float normalizedHorizontal, float normalizedVertical);
	void adjustServoPositions(float normDeltaHorizontal, float normDeltaVertical);
	void CameraDrive(float servo);
	void Autonomous(void); 
	void OperatorControl(void);
	void DriveCode();
	void SetMotorData();
	void GetStickData();
	void LiftCode();
	void RollerCode();
	void TractionControl();
	//void CalibrateTorqueLimits(); 
	float TorqueLimiter(float input_cmd, float motor_rate_norm , float cmd_limit_pos, float cmd_limit_neg, float ratio);
	//void ModeLogic();
	//float HeadingHold();
	void StepCmd();
	
	
	static const float gearRatio = 12.5;			//Andy Mark toughbox ratio
	static const float Ke = 0.021142234;			//CIM-001 Motor back EMF constant in volts/(radian/second)
	float maxWheelRate;
	
	// TODO: Possibly make public for camera autonomous, or move into NeuroAuto
	float horizontalDestination;
	float verticalDestination;
	float horizontalPosition, verticalPosition;	
	float horizontalServoPosition, verticalServoPosition;
	float servoDeadband;			// percentage servo delta to trigger move
	int framesPerSecond;			// number of camera frames to get per second
	float panControl;				// to slow down pan
	float panPosition;
	double sinStart;
	static double panStartTime;
		
	TrackingThreshold td1, td2;		// color thresholds
	ParticleAnalysisReport par1, par2;	// particle analysis report
	ColorReport cReport, cReport2;		// color report	     
	
	float deadzone;
	float slope;
	float intercept;
	
	float headingRef;
	bool cal_switch_on;								//normal value: false	Determines if we are in limit calibration mode 
		
	/////CONTROL PARAMETERS/////
	static const bool sw_headingHold_on = false;	//normal value: true	Enables headingHold function
	static const float Kd = .01;
	static const float Kp = 0.;
	
	///////TORQUE CONTROL///////
	static const bool sw_use_ref_wheel = true;				//normal value: true	Tells us if reference wheels are being used
	static const float tq_limit_ratio = 0.8;				//normal value: 0.8		Ratio for limiting torque
	
	//////TRACTION CONTROL//////
	static const bool sw_compensate_battery = true;
	static const float tol = 0.05;
	static const float gain = 0.10;
};

#endif
